///////////////////////////////////
// 
//  DotLocate.cpp
//
//  Wiicussion
//  HCI 575 Class project
//  Spring 2007
// 
//  Authors:  Jeremy Bennett
//            Adam Faeth
//            Michael Oren
//
///////////////////////////////////////////////////////////

// Local includes
#include "stdafx.h"
#include "DotLocate.h"

// System includes
#include <iostream>
#include <math.h>


/////////////////////////////////////////////
// DotLocate members:
/////////////////////////////////////////////

// Constructor, initialize the environment
DotLocate::DotLocate(float fMaxHeight, float fMaxDist, float fPixelCm)
:   _fMaxHeight(fMaxHeight), // The maximum possible height/space between two dots
    _fMaxDist(fMaxDist),     // The distance in pixels out of the screen when 
                             // the dots are 1 pixel apart
    _fPxCm(fPixelCm)         // The conversion from pixels to cm
{
    // Do nothing
}


// destructor
DotLocate::~DotLocate(void)
{
    // Do nothing
}


// Returns the location based on the input of 4 dots
bool DotLocate::locate(const Dot &rLeftBottom, const Dot &rRightBottom, 
                       const Dot &rRightTop,   const Dot &rLeftTop, 
                       Location &rLoc)
{
    // Calculate the 4 edge lenghts
    float fTop = _dist(rRightTop, rLeftTop);
    float fBottom = _dist(rRightBottom, rLeftBottom);
    float fLeft = _dist(rLeftTop, rLeftBottom);
    float fRight = _dist(rRightTop, rRightBottom);
    
    // hold the two z-coords
    float fZxz, fZyz;
    
    // Calclulate the X and Z coordinates (left-right, near-far)
    twoAxisLoc(fTop, fLeft, fRight, rLoc.pos.fX, fZxz);
    
    // Calculate the Y and Z coordinates (top-bottom, near-far)
    twoAxisLoc(fLeft, fTop, fBottom, rLoc.pos.fY, fZyz);
    
    // Calculate the mean of the Z coordinates
    rLoc.pos.fZ = (fZyz + fZxz)  * 0.5f;
    float fVar = (fZyz - rLoc.pos.fZ) * (fZyz - rLoc.pos.fZ) 
                  + (fZxz - rLoc.pos.fZ) * (fZxz - rLoc.pos.fZ);
    
    std::cout<< "var:  " << fVar << std::endl;
    
    // Determine the "point at" pLocation:
    // the mean of all the X values and all the Y values
    rLoc.point.fX = (rLeftBottom.fX + rRightBottom.fX 
                     + rRightTop.fX + rLeftTop.fX) * 0.25f;
    rLoc.point.fY = (rLeftBottom.fY + rRightBottom.fY 
                     + rRightTop.fY + rLeftTop.fY) * 0.25f;
                     
    // Transform the pixel distances to cm distances:
    rLoc.pos.fX *= _fPxCm;
    rLoc.pos.fY *= _fPxCm;
    rLoc.pos.fZ *= _fPxCm;
    
    return true;
}


// Calculates 2 axis of a location from 3 edges of a known square.
//
// fDistA - distance between two opposite sides (edges) of the square
// fDistB, fDistB - distances from location to opposite sides of the square
// fX     - where to put the first coord (roughly perp to the distance)
// fY     - where to put the second coord (roughly parallel to the distance)
bool DotLocate::twoAxisLoc(const float &fDistA, 
                           const float &fDistB, 
                           const float &fDistC, 
                           float &fX, 
                           float &fY) 
{
    // Calculate distances measured for each side
    float fb = -1.0f * _fMaxDist/_fMaxHeight * fDistB + _fMaxDist;
    float fc = -1.0f * _fMaxDist/_fMaxHeight * fDistC + _fMaxDist;
    
    // Find angle C = arc cos ( (a^2 + b^2 + c^2)/ 2ab)
    float fAngleC = acos( (fDistA*fDistA + fb*fb - fc*fc)/(2.0f*fDistA*fb) );
    
    // Determine X coord (distance right-left or top-bottom of the IR-array)
    // This one is calculated with the top edge, bottom may give another
    fX = -1.0f * (fb * cos(fAngleC) - (0.5f * fDistA) );
    
    // Determine Y coordinate (distance near-far from the IR-Array
    fY = fb * sin(fAngleC);
    
    return true;
}


// Prints out the posiiton information
void DotLocate::print(Location & rLoc)
{
    std::cout << "Position: (" << rLoc.pos.fX << ", " << rLoc.pos.fY << ", " 
              << rLoc.pos.fZ << ")   Pointing At: (" << rLoc.point.fX << ", " 
              << rLoc.point.fY << ")\n";
}



// returns the distance between two Dots
float DotLocate::_dist(const Dot &rLhs, const Dot &rRhs) const 
{
    float fDisX = rLhs.fX - rRhs.fX;
    float fDisY = rLhs.fY - rRhs.fY;
    return sqrt( (fDisX*fDisX) + (fDisY*fDisY) );
}

